#!/usr/bin/python
# -*- coding: utf-8 -*-

from woo import utils
import random
from woo import ymport

## PhysicalParameters 

## Variant of mesh
mesh = 'coarse'
#mesh = 'fine'
#mesh = 'tiny'

## Import geometry 
rod = O.bodies.append(ymport.stl('rod-'+mesh+'.stl',wire=True))

# Spheres
sphereRadius = 0.01
nbSpheres = (32,11,32) 
print("Creating %d spheres..."%(nbSpheres[0]*nbSpheres[1]*nbSpheres[2]), end=' ')
for i in range(nbSpheres[0]):
    for j in range(nbSpheres[1]):
        for k in range(nbSpheres[2]):
            x = (i*2 - nbSpheres[0])*sphereRadius*1.1+sphereRadius*random.uniform(-0.1,0.1)
            y = -j*sphereRadius*2.2-0.01
            z = (k*2 - nbSpheres[2])*sphereRadius*1.1+sphereRadius*random.uniform(-0.1,0.1)
            r = random.uniform(sphereRadius,sphereRadius*0.9)
            dynamic = True
            color=[0.51,0.52,0.4]
            if (i==0 or i==nbSpheres[0]-1 or j==nbSpheres[1]-1 or k==0 or k==nbSpheres[2]-1):
                dynamic = False
                color=[0.21,0.22,0.1]
            O.bodies.append(utils.sphere([x,y,z],r,color=color,dynamic=dynamic))
print("done\n")

## Estimate time step
#O.dt=utils.PWaveTimeStep()
O.dt=0.0001


## Engines 
O.engines=[
    ## Resets forces and momenta the act on bodies
    ForceResetter(),

    ## Using bounding boxes find possible body collisions.
    InsertionSortCollider([
        Bo1_Sphere_Aabb(),
        Bo1_Facet_Aabb(),
    ]),
    InteractionLoop(
        #[Ig2_Sphere_Sphere_Dem3DofGeom(),Ig2_Facet_Sphere_Dem3DofGeom()],
        [Ig2_Sphere_Sphere_ScGeom(),Ig2_Facet_Sphere_ScGeom()],
        [Ip2_FrictMat_FrictMat_FrictPhys()],
        #[Law2_Dem3DofGeom_FrictPhys_CundallStrack()],
        [Law2_ScGeom_FrictPhys_CundallStrack()]
    ),
    ## Apply gravity
    GravityEngine(gravity=[0,-9.81,0]),
    ## Motion equation
    NewtonIntegrator(damping=0.3),
    ## Apply kinematics to rod
    TranslationEngine(ids=rod,translationAxis=[0,-1,0],velocity=0.075),
    ## Save force on rod
    #ForceRecorder(ids=rod,file='force-'+mesh+'.dat',iterPeriod=50),    
]


import sys,time

print("Start simulation: " + mesh)
nbIter=10000

from woo import qt
qt.View()

O.stopAtIter=nbIter
O.run()

#for t in xrange(2):
#    start=time.time();O.run(nbIter);O.wait();finish=time.time() 
#    speed=nbIter/(finish-start); print '%g iter/sec\n'%speed
#print "FINISH"
#quit()

